package prawnBot;

import lejos.nxt.Motor;
import lejos.nxt.Button;

/**
 * Contains all methods related to searching for and finding a pallet.
 * 
 * @author Stephane Beniak - 260324952
 * 
 * @see Claw
 * @see Navigation
 * @see USPoller
 * @see LCDInfo
 */
public class BlockFinder {
   
   /** Defines the minimum obstacle clearance needed for the robot, in cm. */
   public static final int CLEARANCE_FAR = 25;
   /** Defines the minimum reliable US sensor distance, in cm. */
   public static final int CLEARANCE_NEAR = 10;
   /** Defines the minimum distance difference required to recognize a pallet from a wall, in cm. */
   public static final int BLOCK_WALL_DIFF = 15;
   /** Defines the angle correction required for the sweep to perfectly align with a pallet, in degrees. */
   public static final int ANGLE_CORRECT = -1;
   
   private TwoWheeledRobot robot;
   private Odometer odo;
   private USPoller usp;
   
   /**
    * Creates a new blockfinder, with all its relevant parameters.
    * 
    * @param prawnBot The robot that will be block finding.
    * @param odo_ The odometer tracking the robot's movement.
    * @param usp_ The robot's ultrasonic polling system.
    */
   public BlockFinder(TwoWheeledRobot prawnBot, Odometer odo_, USPoller usp_){
      robot = prawnBot;
      odo = odo_;
      usp = usp_;
   }
   
   /**
    * This method sweeps through a specified angle to roughly
    * locate a pallet in the vicinity of the robot.
    * 
    * @param angle The angle to be swept, in degrees.
    * 
    * @return The distance of the pallet from the robot. 0 if no pallet is found.
    */
   public int sweepAngle(double angle){
      
      int ang = (int)(odo.getTheta());
      
      int [] distances = {0, 0, 0};
      
      // Set rotation speed and turn given angle while checking the us sensor.
      robot.setSpeeds(0, Navigation.RSPEED);
      Navigation.turn(odo, angle);
      
      // While the robot's turning, check its recorded us distance. Stop if less than 40cm.
      while((Motor.A.isMoving() || Motor.B.isMoving()) && usp.getDistDiff() < BLOCK_WALL_DIFF);
      robot.setSpeeds(0, 0);
      
      Beeper.two();
      
      // Turn slightly more towards the block
      Navigation.turnRelative(odo, 3);
      
      // If the sweep was interrupted by a pallet, get its distance.
      // Else, no block found, return 0;
      if (usp.getDistDiff() > BLOCK_WALL_DIFF) {
         
         // Get 3 distances. Sleep a little between each ping.
         for (int i = 0; i < 3; i++){
            Navigation.turnRelative(odo, 2);
            try { Thread.sleep(100); } catch (Exception e) {}
            distances[i] = usp.getDistLo();
         }
         
         // Return the average of the 3 distances.
         return (distances[0] + distances[1] + distances[2])/3;
         
      } else {
         return 0;
      }
      
   }
   
   /**
    * Sweeps a narrow area to accurately locate the brick.
    * This method assumed that the robot is currently 
    * roughly facing the brick.
    * 
    * @param filter The brick's measured distance.
    * @param tolerance The width of the sweep's tolerance band, relative to the filter.
    * 
    * @return The distance of the block. -1 if not found.
    */
   public boolean sweepNarrow(int filter, int tolerance){
      
      int angleA, angleB, angleC;
      
      int upperBound = filter + tolerance;
      int lowerBound = filter - tolerance;
      
      // Set this arbitrary distance variable to more than the tolerance.
      // This is used purely for error-checking if the robot's
      // final orientation is not towards the pallet.
      int dist = filter + (2 * tolerance);
      
      // Turn 40 degrees ccw
      robot.setSpeeds(0, Navigation.RSPEED);
      Navigation.turnRelative(odo, -40);
      
      // Sweep slowly clockwise for 90 degrees
      robot.setSpeeds(0, Navigation.RSPEED/2);
      Navigation.turn(odo, 90);
      
      // When the block is first detected, latch its angle and beep
      while (((usp.getDistLo() > upperBound) || (usp.getDistLo() < lowerBound)) 
                && (usp.getDistDiff() < BLOCK_WALL_DIFF));
      
      angleA = (int)odo.getTheta();
      
      Beeper.one();
      
      // Wait for the rotation to finish.
      try { Thread.sleep(2500); } catch (Exception e) {}
      
      // Sweep slowly, ccw, for 90 degrees
      robot.setSpeeds(0, -Navigation.RSPEED/2);
      Navigation.turn(odo, -90);
      
      // When the block is first detected, latch its angle and beep
      while (((usp.getDistLo() > upperBound) || (usp.getDistLo() < lowerBound)) 
                && (usp.getDistDiff() < BLOCK_WALL_DIFF));
      
      angleB = (int)odo.getTheta();
      
      Beeper.one();
      
      // Wait for the rotation to finish.
      try { Thread.sleep(2500); } catch (Exception e) {}
      
      // Correct both angles to a range of [-180, 180], which helps for final angle calculation
      if (angleA > 180)
         angleA -= 360;
      
      if (angleB > 180)
         angleB -= 360;
      
      // Find final block angle, the middle of both latched edges, and turn to it.
      angleC = (angleA + angleB) / 2;
      
      Navigation.turnTo(odo, angleC + ANGLE_CORRECT);
      
      // If, after all this, the robot does not detect a block in front of it, it restarts the sweep()
      try { Thread.sleep(250); } catch (Exception e) {}
      dist = usp.getDistLo();
      
      if ((dist < upperBound) && (dist > lowerBound))
         return true;
      else
         return false;
      
   }
   
   /**
    * The robot approaches the pallet until it is just far enough
    * that it can still rotate without touching it with its claws.
    */
   public void approachFar() {
      
      // Move up to the block. Stop when within far clearance.
      robot.setSpeeds(Navigation.FSPEED, 0);
      while (usp.getDistLo() > CLEARANCE_FAR);
      
      robot.setSpeeds(0, 0);
   }
   
   /**
    * The robot approaches the pallet until it is nearly touching it,
    * ready to perform fixPallet() to straighten the pallet.
    */
   public void approachNear() {
      
      // Move up to the block. Stop when within near clearance.
      robot.setSpeeds(Navigation.FSPEED, 0);
      Navigation.goForward(odo, CLEARANCE_FAR - CLEARANCE_NEAR);
      
      robot.setSpeeds(0, 0);
      
      Beeper.two();
      
      // Advance to nearly touch the block
      robot.setSpeeds(Navigation.FSPEED/2, 0);
      Navigation.goForward(odo, CLEARANCE_NEAR - 1);
   }
   
   /**
    * Moves the robot in an S shaped pattern, such that a pallet will be straightened out,
    * if it happens to be at a strange angle. This is to ease pallet pickup.
    */
   public void fixPallet() {
      
      Navigation.turnAboutWheel(odo, 25, true);
      Navigation.turnAboutWheel(odo, 25, false);
      
      robot.setSpeeds(Navigation.FSPEED, 0);
      Navigation.goForward(odo, 5);
      Navigation.goForward(odo, -3);
      
      Claw.liftLow();
      
      Navigation.goForward(odo, -5);
      Claw.release();
      
      Navigation.goForward(odo, 5);
      Navigation.goForward(odo, -2.5);
      
   }
   
   /**
    * Begins a pallet search over the specified angle. If a block is found
    * it is swept for, located, approached, straightened, and finally grabbed.
    * The block is assumed to be roughly ahead of the robot.
    * 
    * @param angle The angle of the pallet search, in degrees.
    * @param straight If false, the block will be grabbed diagonally (45 degrees).
    */
   public boolean getPallet(double angle, boolean straight){
      
      // block status booleans
      boolean blockFound1 = true, blockFound2 = true, blockFound3 = false;
      int dist = 25;
      
      // Attempt to get it 2 times
      for(int i = 0; i < 2 && !blockFound3; i++) {
         
         blockFound1 = sweepNarrow(dist, 10);
         
         // If a pallet was confirmed once...
         if (blockFound1) {
            
            Beeper.major();
            
            // If the pallet is very far, approach up to max claw clearance
            // and sweep again, so pallet is more precisely located.
            if (dist > CLEARANCE_FAR){
               approachFar();
               dist = usp.getDistLo();
               blockFound2 = sweepNarrow(dist, 7);
            }
            
            // If the block is confirmed again, go and grab it
            if (blockFound2) {
               
               // If true, grab it straight, else diagonally
               if (straight) {
                  approachNear();
                  fixPallet();
                  
               } else {
                  
                  // advance / back up to the right distance.
                  int dist1, dist2, dist3;
                  
                  // Get 3 dists, then get their average, for accuracy
                  dist1 = usp.getDistLo();
                  try { Thread.sleep(100); } catch (Exception e) {}
                  dist2 = usp.getDistLo();
                  try { Thread.sleep(100); } catch (Exception e) {}
                  dist3 = usp.getDistLo();
                  
                  dist3 = (dist1 + dist2 + dist3) / 3;
                  dist1 = dist3 - CLEARANCE_FAR;
                  
                  Navigation.goForward(odo, dist1);
                  
                  
                  // Move to the diagonal
                  robot.setSpeeds(0, Navigation.RSPEED);
                  Navigation.turnRelative(odo, 90);
                  
                  boolean rightBlocked = false, leftBlocked = false;
                  
                  // Go left if you can't go right.
                  if (usp.getDistHi() < 50) {
                     Navigation.turnRelative(odo, 180);
                     rightBlocked = true;
                     
                     // Turn around if you can't get either way.
                     if (usp.getDistHi() < 50) {
                        Navigation.turnRelative(odo, 180);
                        leftBlocked = true;
                        
                        if (usp.getDistHi() > 50) {
                           rightBlocked = false;
                        } else {
                           Navigation.turnRelative(odo, 90);
                        }
                        
                     }
                  }
                  
                  // If either side is clear.
                  if (!rightBlocked || !leftBlocked) {
                     
                     robot.setSpeeds(Navigation.FSPEED, 0);
                     Navigation.goForward(odo, 38);
                     
                     // Approach to grab
                     robot.setSpeeds(0, Navigation.RSPEED);
                     
                     Navigation.turnRelative(odo, (rightBlocked ? 135 : -135));
                     
                     robot.setSpeeds(Navigation.FSPEED, 0);
                     Navigation.goForward(odo, 35.74);
                     
                  }
                  
               }
               
               // Grab the pallet
               Claw.liftLow();
               blockFound3 = true;
               Main.pallet = true;
               
            }
            
            // If the first attempt failed, try again once more...
         } else {
            
            if (i < 1) {
               Navigation.turnRelative(odo, -45);
               dist = sweepAngle(90);
            }
            
         }
         
      }
      
      return blockFound3;
      
   }
   
}